#pragma once


#include <Math/Matrix44f.hpp>
#include <Math/Constants.hpp>


// Simple helper function.
inline bool MatrixEqual( const Math::Matrix44f& lhs, const Math::Matrix44f& rhs )
{
	for( int i=0; i<4; ++i )
	{
		for( int j=0; j<4; ++j )
		{
			float	delta	=	std::abs( lhs( i )( j )-rhs( i )( j ) );
			if( delta > 0.001f )
				return false;
		}
	}

	return true;
}


inline bool				VectorEqual( const Math::Vector3f& lhs, const Math::Vector3f& rhs )
{
	// Simple, dirty and not very good.
	Math::Vector3f	delta	=	lhs - rhs;
	return delta.X()<0.01f && delta.Y()<0.01f && delta.Z()<0.01f;
}


inline bool				VectorEqual( const Math::Vector4f& lhs, const Math::Vector4f& rhs )
{
	// Simple, dirty and not very good.
	Math::Vector4f	delta	=	lhs - rhs;
	return delta.X()<0.01f && delta.Y()<0.01f && delta.Z()<0.01f && delta.W()<0.01f;
}


// Quick helper for visualizations.  Just zaps any insane small numbers
// caused by float rounding.
inline Math::Matrix44f Unitize( Math::Matrix44f& m )
{
	Math::Matrix44f	result;
	for( int i=0; i<4; ++i )
	{
		Math::Vector4f	part;
		for( int j=0; j<4; ++j )
		{
			part( j, (m( i )( j )<0.001f) && (m( i )( j )>-0.001f) ? 0.0f : m( i )( j ) );
		}
		result( i, part );
	}
	return result;
}


/*
	Matrix's have to be one of the most confusing
	items in 3D work due to a combination of bad
	documentation, confusing conventions, etc.

	We will be following the conventions of Open GL
	in this case.  This has no real meaning to the
	memory layout and as such this class is usable
	by both Open GL and Direct 3D.

	Rotations are counter clockwise around the axis
	of rotation.
*/

TEST( Matrix44f, ConstructionAndAccess )
{
	using Math::Matrix44f;

	Matrix44f	test	=	Matrix44f::Identity();
	EXPECT_FLOAT_EQ( test( 0 )( 0 ), 1.0f );
	EXPECT_FLOAT_EQ( test( 1 )( 0 ), 0.0f );
	EXPECT_FLOAT_EQ( test( 2 )( 0 ), 0.0f );
	EXPECT_FLOAT_EQ( test( 3 )( 0 ), 0.0f );

	EXPECT_FLOAT_EQ( test( 0 )( 1 ), 0.0f );
	EXPECT_FLOAT_EQ( test( 1 )( 1 ), 1.0f );
	EXPECT_FLOAT_EQ( test( 2 )( 1 ), 0.0f );
	EXPECT_FLOAT_EQ( test( 3 )( 1 ), 0.0f );

	EXPECT_FLOAT_EQ( test( 0 )( 2 ), 0.0f );
	EXPECT_FLOAT_EQ( test( 1 )( 2 ), 0.0f );
	EXPECT_FLOAT_EQ( test( 2 )( 2 ), 1.0f );
	EXPECT_FLOAT_EQ( test( 3 )( 2 ), 0.0f );

	EXPECT_FLOAT_EQ( test( 0 )( 3 ), 0.0f );
	EXPECT_FLOAT_EQ( test( 1 )( 3 ), 0.0f );
	EXPECT_FLOAT_EQ( test( 2 )( 3 ), 0.0f );
	EXPECT_FLOAT_EQ( test( 3 )( 3 ), 1.0f );
}


TEST( Matrix44f, Transposition )
{
	using Math::Matrix44f;
	using Math::Vector4f;

	Matrix44f	test(
		1,	2,	3,	4,
		5,	6,	7,	8,
		9,	10,	11,	12,
		13,	14,	15,	16	);
	Matrix44f	transposed	=	test;
	transposed.Transpose();

	EXPECT_TRUE( VectorEqual( transposed( 0 ), Vector4f( 1, 2, 3, 4 ) ) );
	EXPECT_TRUE( VectorEqual( transposed( 1 ), Vector4f( 5, 6, 7, 8 ) ) );
	EXPECT_TRUE( VectorEqual( transposed( 2 ), Vector4f( 9, 10, 11, 12 ) ) );
	EXPECT_TRUE( VectorEqual( transposed( 3 ), Vector4f( 13, 14, 15, 16 ) ) );
}


TEST( Matrix44f, AxisRotations )
{
	using Math::Matrix44f;
	using Math::Vector4f;

	// Test X rotations.
	{
		// Rotates y axis into +z axis.
		// Rotates z axis into -y axis.
		Matrix44f	xRotation	=	Matrix44f::RotateX( Math::Constants< float >::HalfPi() );
		EXPECT_TRUE( VectorEqual( xRotation( 1 ), Vector4f::ZAxis() ) );
		EXPECT_TRUE( VectorEqual( xRotation( 2 ), -Vector4f::YAxis() ) );
	}

	// Test Y rotations.
	{
		// Rotates x axis into -z axis.
		// Rotates z axis into +x axis.
		Matrix44f	yRotation	=	Matrix44f::RotateY( Math::Constants< float >::HalfPi() );
		EXPECT_TRUE( VectorEqual( yRotation( 0 ), -Vector4f::ZAxis() ) );
		EXPECT_TRUE( VectorEqual( yRotation( 2 ), Vector4f::XAxis() ) );
	}

	// Test Z rotations.
	{
		// Rotates x axis into +y axis.
		// Rotates y axis into -x axis.
		Matrix44f	zRotation	=	Matrix44f::RotateZ( Math::Constants< float >::HalfPi() );
		EXPECT_TRUE( VectorEqual( zRotation( 0 ), Vector4f::YAxis() ) );
		EXPECT_TRUE( VectorEqual( zRotation( 1 ), -Vector4f::XAxis() ) );
	}
}


TEST( Matrix44f, ArbitraryRotation )
{
	using Math::Matrix44f;
	using Math::Vector3f;

	// NOTE: Assumes axis rotations have been tested and proven correct.
	// Use an arbitrary angle for fun.
	{
		Matrix44f	xRotation	=	Matrix44f::RotateX( 1.0f );
		Matrix44f	aRotation	=	Matrix44f::Rotate( Vector3f::XAxis(), 1.0f );

		EXPECT_TRUE( MatrixEqual( xRotation, aRotation ) );
	}
	{
		Matrix44f	yRotation	=	Matrix44f::RotateY( 1.0f );
		Matrix44f	aRotation	=	Matrix44f::Rotate( Vector3f::YAxis(), 1.0f );

		EXPECT_TRUE( MatrixEqual( yRotation, aRotation ) );
	}
	{
		Matrix44f	zRotation	=	Matrix44f::RotateZ( 1.0f );
		Matrix44f	aRotation	=	Matrix44f::Rotate( Vector3f::ZAxis(), 1.0f );

		EXPECT_TRUE( MatrixEqual( zRotation, aRotation ) );
	}
}


TEST( Matrix44f, IdentityMultiply )
{
	using Math::Matrix44f;
	using Math::Vector3f;
	using Math::Vector4f;

	// Test matrix versus matrix.
	{
		Matrix44f	result	=	Matrix44f::Identity() * Matrix44f::Identity();
		EXPECT_TRUE( MatrixEqual( Matrix44f::Identity(), result ) );
	}

	// Test matrix versus vector 3.
	{
		EXPECT_TRUE( VectorEqual( Vector3f::XAxis(), Matrix44f::Identity() * Vector3f::XAxis() ) );
		EXPECT_TRUE( VectorEqual( Vector3f::YAxis(), Matrix44f::Identity() * Vector3f::YAxis() ) );
		EXPECT_TRUE( VectorEqual( Vector3f::ZAxis(), Matrix44f::Identity() * Vector3f::ZAxis() ) );
	}

	// Test matrix versus vector 4.
	{
		EXPECT_TRUE( VectorEqual( Vector4f::XAxis(), Matrix44f::Identity() * Vector4f::XAxis() ) );
		EXPECT_TRUE( VectorEqual( Vector4f::YAxis(), Matrix44f::Identity() * Vector4f::YAxis() ) );
		EXPECT_TRUE( VectorEqual( Vector4f::ZAxis(), Matrix44f::Identity() * Vector4f::ZAxis() ) );
		EXPECT_TRUE( VectorEqual( Vector4f::WAxis(), Matrix44f::Identity() * Vector4f::WAxis() ) );
	}
}


TEST( Matrix44f, RotationMultiplyTests )
{
	using Math::Matrix44f;
	using Math::Vector3f;
	using Math::Vector4f;

	// Test X rotations.
	{
		// Rotates y axis into +z axis.
		// Rotates z axis into -y axis.
		Matrix44f	xRotation	=	Matrix44f::RotateX( Math::Constants< float >::HalfPi() );
		EXPECT_TRUE( VectorEqual( xRotation * Vector3f::YAxis(), Vector3f::ZAxis() ) );
		EXPECT_TRUE( VectorEqual( xRotation * Vector3f::ZAxis(), -Vector3f::YAxis() ) );

		EXPECT_TRUE( VectorEqual( xRotation * Vector4f::YAxis(), Vector4f::ZAxis() ) );
		EXPECT_TRUE( VectorEqual( xRotation * Vector4f::ZAxis(), -Vector4f::YAxis() ) );
	}

	// Test Y rotations.
	{
		// Rotates x axis into -z axis.
		// Rotates z axis into +x axis.
		Matrix44f	yRotation	=	Matrix44f::RotateY( Math::Constants< float >::HalfPi() );
		EXPECT_TRUE( VectorEqual( yRotation * Vector3f::XAxis(), -Vector3f::ZAxis() ) );
		EXPECT_TRUE( VectorEqual( yRotation * Vector3f::ZAxis(), Vector3f::XAxis() ) );

		EXPECT_TRUE( VectorEqual( yRotation * Vector4f::XAxis(), -Vector4f::ZAxis() ) );
		EXPECT_TRUE( VectorEqual( yRotation * Vector4f::ZAxis(), Vector4f::XAxis() ) );
	}

	// Test Z rotations.
	{
		// Rotates x axis into +y axis.
		// Rotates y axis into -x axis.
		Matrix44f	zRotation	=	Matrix44f::RotateZ( Math::Constants< float >::HalfPi() );
		EXPECT_TRUE( VectorEqual( zRotation * Vector3f::XAxis(), Vector3f::YAxis() ) );
		EXPECT_TRUE( VectorEqual( zRotation * Vector3f::YAxis(), -Vector3f::XAxis() ) );

		EXPECT_TRUE( VectorEqual( zRotation * Vector4f::XAxis(), Vector4f::YAxis() ) );
		EXPECT_TRUE( VectorEqual( zRotation * Vector4f::YAxis(), -Vector4f::XAxis() ) );
	}
}


TEST( Matrix44f, ScaleTests )
{
	using Math::Matrix44f;
	using Math::Vector4f;

	// Test general layout.
	{
		Matrix44f	layout	=	Matrix44f::Scale( 1.0f, 2.0f, 3.0f );

		// Inspect the matrix elements directly.
		EXPECT_FLOAT_EQ( 1.0f, layout( 0 )( 0 ) );
		EXPECT_FLOAT_EQ( 2.0f, layout( 1 )( 1 ) );
		EXPECT_FLOAT_EQ( 3.0f, layout( 2 )( 2 ) );
	}

	// Test half scale.
	{
		Matrix44f	halfScale	=	Matrix44f::Scale( 0.5f, 0.5f, 0.5f );

		EXPECT_FLOAT_EQ( 0.5f, (halfScale * Vector4f::XAxis()).X() );
		EXPECT_FLOAT_EQ( 0.5f, (halfScale * Vector4f::YAxis()).Y() );
		EXPECT_FLOAT_EQ( 0.5f, (halfScale * Vector4f::ZAxis()).Z() );
	}

	// Test double scale.
	{
		Matrix44f	doubleScale	=	Matrix44f::Scale( 2.0f, 2.0f, 2.0f );

		EXPECT_FLOAT_EQ( 2.0f, (doubleScale * Vector4f::XAxis()).X() );
		EXPECT_FLOAT_EQ( 2.0f, (doubleScale * Vector4f::YAxis()).Y() );
		EXPECT_FLOAT_EQ( 2.0f, (doubleScale * Vector4f::ZAxis()).Z() );
	}
}


TEST( Matrix44f, Translation )
{
	using Math::Matrix44f;
	using Math::Vector3f;

	// Translate by 5, 6, 7.
	Vector3f	testVec			=	Vector3f( 1, 2, 3 );
	Matrix44f	translation		=	Matrix44f::Translation( 5, 6, 7 );

	Vector3f	result			=	translation * testVec;

	EXPECT_TRUE( VectorEqual( result, Vector3f( 6, 8, 10 ) ) );
}


TEST( Matrix44f, Inversion )
{
	using Math::Matrix44f;

	Matrix44f	invIdent	=	Matrix44f::Identity();
	invIdent.Inverse();

	EXPECT_FLOAT_EQ( 1.0f, invIdent( 0 )( 0 ) );
	EXPECT_FLOAT_EQ( 1.0f, invIdent( 1 )( 1 ) );
	EXPECT_FLOAT_EQ( 1.0f, invIdent( 2 )( 2 ) );
	EXPECT_FLOAT_EQ( 1.0f, invIdent( 3 )( 3 ) );

	Matrix44f	invTrans	=	Matrix44f::Translation( 5, 6, 7 );
	invTrans.Inverse();

	EXPECT_FLOAT_EQ( -5.0f, invTrans( 3 )( 0 ) );
	EXPECT_FLOAT_EQ( -6.0f, invTrans( 3 )( 1 ) );
	EXPECT_FLOAT_EQ( -7.0f, invTrans( 3 )( 2 ) );
	EXPECT_FLOAT_EQ(  1.0f, invIdent( 3 )( 3 ) );

	Matrix44f	invScale	=	Matrix44f::Scale( 2, 4, 8 );
	invScale.Inverse();

	EXPECT_FLOAT_EQ(   0.5f, invScale( 0 )( 0 ) );
	EXPECT_FLOAT_EQ(  0.25f, invScale( 1 )( 1 ) );
	EXPECT_FLOAT_EQ( 0.125f, invScale( 2 )( 2 ) );
	EXPECT_FLOAT_EQ(   1.0f, invScale( 3 )( 3 ) );
}
